#include "MotorJoystick.h"

SoftwareSerial mySerial =  SoftwareSerial(rxPin, txPin);// read to USB

void JoystickMotor::setup(){

pinMode(JMotorPin1, OUTPUT); // set enable as output pin
pinMode(JMotorPin2, OUTPUT); // set x Analog pin 1
pinMode(JMotorPin3, OUTPUT); // set x Analog pin 2
pinMode(JMotorPin4, OUTPUT); // set y Analog pin 1
pinMode(JMotorPin5, OUTPUT); // set y Analog pin 2

pinMode(xPin, INPUT);
pinMode(yPin, INPUT);

digitalWrite(txPin, HIGH);


mySerial.begin(MAXBAUDRATE); // intialize serial baud rate

mySerial.println("Welcome");

X_status = r_brake;
Y_status = r_brake;

Wire.begin();
Wire.beginTransmission(0x09);
Wire.send('c');
	Wire.send(0x00);
	Wire.send(0xff);
	Wire.send(0xff);
	Wire.endTransmission();

}

void JoystickMotor::loop()
{

	AnalogVal1 = analogRead(AnalogPin0); // read in x
	AnalogVal2 = analogRead(AnalogPin1); // read in y


	if( AnalogVal1 != prevVal1 )
		isJoystickMoved = true;
	if ( AnalogVal2 != prevVal2 )
		isJoystickMoved = true;

// XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX Make it into a function later XXXXXXXXXXXXXXXXXXXXXXXXXXX
	// Movement on Joystick for X
	if (isJoystickMoved == true)
	{
		// set enable pin to high
		digitalWrite(JMotorPin1, HIGH);

		if(AnalogVal1 < DC_MOTOR_LOW_POINT)
		{
			// DC Result backward
			digitalWrite(JMotorPin2, HIGH);
			digitalWrite(JMotorPin3, LOW);

			X_status = r_back; // change status of X joystick
		}
		else if (AnalogVal1 > DC_MOTOR_HIGH_POINT)
		{
			// DC Result to Forward
			digitalWrite(JMotorPin2, LOW);
			digitalWrite(JMotorPin3, HIGH);

			X_status = r_forward;
		}
		else
		{
			// J result to brake
			digitalWrite(JMotorPin2, LOW);
			digitalWrite(JMotorPin3, LOW);

			X_status = r_brake;
		}

		// Movement on Joystick for y
		if(AnalogVal2 < DC_MOTOR_LOW_POINT)
		{
			// J Result backward
			digitalWrite(JMotorPin4, HIGH);
			digitalWrite(JMotorPin5, LOW);

			Y_status = r_back; // change status of Y joystick
		}
		else if (AnalogVal2 > DC_MOTOR_HIGH_POINT)
		{
			// J Result to Forward
			digitalWrite(JMotorPin4, LOW);
			digitalWrite(JMotorPin5, HIGH);

			Y_status = r_forward;
		}
		else
		{
			// J result to brake
			digitalWrite(JMotorPin4, LOW);
			digitalWrite(JMotorPin5, LOW);

			Y_status = r_brake;
		}

		ChangeBlinkMColor();
		isJoystickMoved = false;
	}
}

void JoystickMotor::ChangeBlinkMColor()
{
	if( (X_status == r_forward) && (Y_status == r_back))
	{
		// Turn Right  Change color to Yellow
		Wire.beginTransmission(0x09);
		Wire.send('C');
		Wire.send(0xff);
		Wire.send(0xff);
		Wire.send(0x00);
		Wire.endTransmission();

		mySerial.println("I AM IN HERE");

	}
	else if ((X_status == r_back) && (Y_status == r_forward))
	{
		// Turn Left Change color to Green
	}
	else if (( X_status == r_forward) && (Y_status == r_forward))
	{
		// Full forward Change color to Purple
	}
	else if (( X_status == r_back) && (Y_status == r_back))
	{
		// Full Back  Change color to RED
	}
	else
	{
		// Brake Change color to Black
	}

}


